/*************************************************
*      Filename: main_text.cu
*        Coding: utf-8
*        Author: Zheng Li
*         Email: lizheng@galasports.net
*      HomePage: http://www.galasports.net
*  Copyright(C): InnoReal Technology Co.,Ltd.
*       Version: 0.0.1
*    CreateDate: 2018-07-20 20:26:39
*    LastChange: 2018-07-20 20:28:53
*          Desc:
*       History:
*************************************************/
#include <innoreal/utils/innoreal_helper.hpp>
#include <innoreal/utils/innoreal_timer.hpp>
#include "input_data.h"
#include "constant.h"
#include "gn_solver.h"
#include "distance_twomesh.h"
//#include "keyframe_find.h"
#include "keyframe_movingpoint.h"
#include <vector>
//#include "output_vector1d2d.h"
//#include "output_vec1d2d_typename.h"
#include <vector_functions.hpp>
#include <igl/cotmatrix.h>
#include <igl/hausdorff.h>
#include <Eigen/Dense>
//#include <Eigen/Sparse>
using namespace Eigen;


class MeshVertex{
 public:
  int num_vertex_;
  std::vector<float3> d_vertex_;
  int num_face_;
  std::vector<int3> d_face_;
};
class TwoMesh{
 public:
  MeshVertex source_;
  MeshVertex target_;
};

void ReadDataFromFile(std::string file, MeshVertex *data) {
  std::ifstream mesh_fn(file);

  std::vector<float3> h_vertex;
  h_vertex.reserve(200000);
  std::vector<int3> h_face;
  h_face.reserve(410000);

  float x, y, z;
  char flag;
  std::string line;
  while (std::getline(mesh_fn, line)) {
    if (line.find('t') != -1) {
      //printf("wrong line find t\n!");
    } else {
      sscanf(line.c_str(), "%c %f %f %f", &flag, &x, &y, &z);
    }
    if ('v' == flag) {
      h_vertex.push_back(make_float3(x, y, z));
    }
    if (('f' == flag)) {
      h_face.push_back(make_int3(x - 1, y - 1, z - 1));
    }
  }
    mesh_fn.close();

    data->num_vertex_ = h_vertex.size();
    data->d_vertex_ = h_vertex;
    data->num_face_ = h_face.size();
    data->d_face_ = h_face;
}

  void FormulateTwoData(std::string src, std::string tar, TwoMesh *data){
    ReadDataFromFile(src, &data->source_);
    ReadDataFromFile(tar, &data->target_);
  }



  double Hausdorff_eigen(TwoMesh *data)
  {
    double distance = 0.0,temp=0;
    std::vector<float3> v_a = data->source_.d_vertex_;
    std::vector<float3> v_b = data->target_.d_vertex_;
    int va_N = data->source_.num_vertex_;
    int vb_N = data->target_.num_vertex_;
    MatrixXd A(vb_N, 3), B(vb_N, 3), C(vb_N,3);
    MatrixXd a(1,3), l(vb_N,1);
    for(int i=0;i<vb_N;i++){
      B(i,0)=v_b[i].x;B(i,1)=v_b[i].y;B(i,2)=v_b[i].z;
    }
    for(int i=0;i<va_N;i++){
      a(0,0)=v_a[i].x;a(0,1)=v_a[i].y;a(0,2)=v_a[i].z;
      A=a.replicate(vb_N,1);
      C=A-B;
      C=C.array()*C.array();
      l=C.rowwise().sum();
      temp=l.minCoeff();
      distance=temp>distance?temp:distance;
    }

    distance=sqrt(distance);
    return distance;
  }


  double Hausdorff_Simple(TwoMesh *data)
  {
    double distance = 0.0;
    thrust::host_vector<float3>  v_a = data->source_.d_vertex_;
    thrust::host_vector<float3>  v_b = data->target_.d_vertex_;
    int va_N = data->source_.num_vertex_;
    int vb_N = data->target_.num_vertex_;
    double dis_temp;
    double dis_temp1 = 0, dis_temp2;

    for(int i = 0; i < va_N; i++){
      dis_temp2 = 1000000;
      for(int j = 0; j < vb_N; j++){
        dis_temp = (v_a[i].x-v_b[j].x)*(v_a[i].x-v_b[j].x) + (v_a[i].y-v_b[j].y)*(v_a[i].y-v_b[j].y)  + (v_a[i].z-v_b[j].z)*(v_a[i].z-v_b[j].z);
        if(dis_temp<dis_temp2){
          dis_temp2=dis_temp;
        }
      }
      if(dis_temp2>dis_temp1){
        dis_temp1=dis_temp2;
      }
    }
    distance=sqrt(dis_temp1);
    return distance;
  }

  int main(int argc, char** argv)
  {

//    KeyFrameFind *keyframe_find = new KeyFrameFind;
    KeyFrameMovingPoint * keyframe_movingpoint = new KeyFrameMovingPoint;
    TwoMesh *input_data=new TwoMesh;
    DisTwoMesh *distmesh = new DisTwoMesh;
    char file_src[100], file_tar[100], file_tar1[100], file_tar2[100];

    int N = atoi(argv[1]);
    int K = atoi(argv[2]);
    int Iter=20;
    int startF=1;
    INNOREALI("Frame_sum: %d    Frame_given: %d \n", N, K);


    std::vector<int> center;//center.resize(K);
    std::vector<double> costL,costR;//ncenter.resize(K);
    std::vector<int> L,R;
    int c1=0,c2=0;
    double dis1=0,dis2=0,dis=0;
    int label[N];
    std::vector<double> cost;
    double d;

    int spacing=N/K;
    for(int i=0;i<K;i++){
      center.push_back(i*spacing);
      costL.push_back(0);
      costR.push_back(0);
      L.push_back(0);
      R.push_back(0);
    }

    for(int ii=0; ii<Iter;ii++) {
      clock_t start, stop;       start = clock();


      for(int i=0;i<K;i++){
        costL[i]=0;costR[i]=0;L[i]=0;R[i]=0;
      }

      for (int i = 0; i < center[0]; i++) {
        sprintf(file_src, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", center[0]+startF);
        sprintf(file_tar, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", i+startF);
        FormulateTwoData(file_src, file_tar, input_data);
        dis = Hausdorff_Simple(input_data);
        label[i] = 1;
        costL[0] += dis;
      }
      L[0] = 0;

      for (int i = center.back(); i < N; i++) {
        sprintf(file_src, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", center[K-1]+startF);
        sprintf(file_tar, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", i+startF);
        FormulateTwoData(file_src, file_tar, input_data);
        dis = Hausdorff_Simple(input_data);
//        std::cout<<"("<<center[K-1]+startF<<","<<i+startF<<"):"<<dis<<" ";
        label[i] = K;
        costR[K - 1] += dis;
      }
      std::cout<<std::endl;
      R[K - 1] = N - 1;

      for (int i = 0; i < K - 1; i++) {
        c1 = center[i];
        c2 = center[i + 1];
        sprintf(file_tar1, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", c1+startF);
        sprintf(file_tar2, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", c2+startF);
        for (int j = c1; j <= c2; j++) {
          sprintf(file_src, "/data/workspace/Offline4D/data/168/mesh/mesh%d.obj", j+startF);
          FormulateTwoData(file_src, file_tar1, input_data);
          dis1 = Hausdorff_Simple(input_data);
          FormulateTwoData(file_src, file_tar2, input_data);
          dis2 = Hausdorff_Simple(input_data);
//          std::cout<<dis1<<","<<dis2<<std::endl;
          if (dis1 < dis2) {
            label[j] = i + 1;
            costR[i] += dis1;
            R[i] = j;
          } else {
            label[j] = i + 2;
            costL[i + 1] += dis2;
            if (L[i + 1] == 0) {
              L[i + 1] = j;
            }
          }
        }
      }

      d = 0;
      for (int i = 0; i < K; i++) {
        d += costL[i] + costR[i];
      }
      cost.push_back(d);
      INNOREALI("cost: %lf\n", d);

//      std::cout<<"L:  ";
//      for(int i=0;i<K;i++){
//        std::cout<<L[i]<<" ";
//      }
//      std::cout<<std::endl;
//      std::cout<<"R:  ";
//      for(int i=0;i<K;i++){
//        std::cout<<R[i]<<" ";
//      }
//      std::cout<<std::endl;


      std::cout<<"Center:  ";
      for (int i = 0; i < K; i++) {
        center[i] = ceil((L[i]+1 + R[i]+1) / 2.0)-1;
        std::cout<<center[i]<<" ";
      }
      std::cout<<std::endl;

      stop = clock();
      INNOREALI("%d iter time: %f s\n", ii, double(stop-start)/CLOCKS_PER_SEC);

      for(int i=0;i<N;i++){
        std::cout<<label[i]<<" ";
      }
      std::cout<<std::endl;

    }

    std::fstream file;
    std::string  fileKeyFrame;
    fileKeyFrame= "data/168/keyframe_csv/keyframeK"+std::to_string(K)+".csv";
    file.open(fileKeyFrame.c_str(),std::ios::out);
    for(int i=0;i<N;i++){
      file<<startF+i<<","<<center[label[i]-1]+startF<<std::endl;
    }
    file.close();

    fileKeyFrame = "data/168/keyframe_csv/costK"+std::to_string(K)+".csv";
    file.open(fileKeyFrame.c_str(),std::ios::out);
    for(int i=0;i<Iter;i++){
      file<<cost[i]<<std::endl;
    }
    file.close();


    delete keyframe_movingpoint;
//    delete keyframe_find;
    return 0;


  }

